#include <src/pcps.h>
#include <src/matrix_algebra.h>
#include <src/blas_lapack.h>

//-------------------------------------------------------------------------------
// pcps::matrix_inverse_lu -- Computes a matrix's inverse and determinant using
//                            an LU decomposition.
//-------------------------------------------------------------------------------

                                                     // exit status    array size      description
template <class S>                              // -----------    ----------      ----------------------------------------------------------------
void pcps::matrix_inverse_lu(const int n,            // unchanged                      dimension of the matrix
                             S & det,           // overwritten                    On exit, the matrix's determinant.
                             S * const A,       // overwritten      n*n           On entry, the matrix to be inverted.  On exit, the inverse.
                             S * const work,    // overwritten      n*n           work space for computing the inverse
                             int * const ipiv,       // overwritten      n             work space for pivoting during the LU decomposition
                             int * const inv_perm)   // overwritten      n             work space for inverting the LU pivoting
{

  // copy the matrix into the workspace
  pcps::xcopy(n*n, A, 1, work, 1);

  // compute the LU decomposition of the matrix
  int info;
  pcps::xgetrf(n, n, work, n, ipiv, info);
  if (info < 0)
    throw pcps::Exception( (boost::format("pcps::xgetrf failed with error code %i in pcps::matrix_inverse_lu") % info).str() );
  if (info > 0)
    throw pcps::SingularMatrixException( (boost::format("U(%i,%i) is exactly zero in pcps::matrix_inverse_lu, thus the matrix is not invertable") % info % info).str() );

  // since FORTRAN counts from 1, subtract 1 from each of the pivot array values
  for (int i = 0; i < n; i++)
    ipiv[i] -= 1;

  // compute the determinant
  det = pcps::unity<S>();
  for (int i = 0; i < n; i++)
    if ( i != ipiv[i] )
      det *= - work[i*n+i];
    else
      det *=   work[i*n+i];
  //if (std::abs(det) < 1.0e-12)
  //  throw pcps::SingularMatrixException( (boost::format("matrix determinant is zero in pcps::matrix_inverse_lu, thus the matrix is not invertable")).str() );

  // determine the inverse of the pivot array's permutation
  for (int i = 0; i < n; i++)
    inv_perm[i] = i;
  for (int i = n-1; i >= 0; i--)
    std::swap(inv_perm[i], inv_perm[ipiv[i]]);

  // compute the inverse of the upper triangle matrix
  for (int i = 0; i < n; i++)
  for (int j = i; j < n; j++) {
    A[i*n+j] = ( i == j ? pcps::unity<S>() : pcps::zero<S>() );
    for (int k = i; k < j; k++)
      A[i*n+j] -= A[i*n+k] * work[j*n+k];  // work indices transposed due to FORTRAN
    A[i*n+j] /= work[j*n+j];
  }

  // compute the inverse of the lower triangle matrix (whose diagonal elements are one and are not stored)
  for (int i = 1; i < n; i++)
  for (int j = i-1; j >= 0; j--) {
    A[i*n+j] = -work[j*n+i];  // work indices transposed due to FORTRAN
    for (int k = j+1; k < i; k++)
      A[i*n+j] -= A[k*n+j] * work[k*n+i];  // work indices transposed due to FORTRAN
  }

  // compute the product of the inverse triangular matrices
  {
    const S * const U_inv = A;
    const S * const L_inv = A;
    for (int i = 0; i < n; i++)
    for (int j = 0; j < n; j++) {
      // term with diagonal of L_inv (which equals one)
      work[i*n+j] = ( i <= j ? U_inv[i*n+j] : pcps::zero<S>() );
      // other terms
      for (int k = std::max(i, j+1); k < n; k++)
        work[i*n+j] += U_inv[i*n+k] * L_inv[k*n+j];
    }
  }

  // Multiply by the inverse of the permutation matrix (column reordering)
  // We also transpose the final matrix to account for FORTRAN indexing
  for (int i = 0; i < n; i++)
  for (int j = 0; j < n; j++)
    A[j*n+i] = work[i*n+inv_perm[j]];

}

// explicitly instantiate the function
template void pcps::matrix_inverse_lu(const int n,
                                      double & det,
                                      double * const A,
                                      double * const work,
                                      int * const ipiv,
                                      int * const inv_perm);
template void pcps::matrix_inverse_lu(const int n,
                                      std::complex<double> & det,
                                      std::complex<double> * const A,
                                      std::complex<double> * const work,
                                      int * const ipiv,
                                      int * const inv_perm);

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the determinant of a matrix using LU decomposition
///
/// \param[in]       n       The matrix dimension.
/// \param[in,out]   A       On entry, the matrix whose determinant we want.  Overwritten on exit. Size n*n.
/// \param[out]      ipiv    Work space for pivoting during the LU decomposition, size n.
///
/// \return the determinant in mantissa/exponent format
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template <class S> pcps::MantExp<S> pcps::matrix_determinant_lu(const int n, S * const A, int * const ipiv) {

  // compute the LU decomposition of the matrix
  int info;
  pcps::xgetrf(n, n, A, n, ipiv, info);
  if (info < 0)
    throw pcps::Exception("pcps::xgetrf failed with error code %i in pcps::matrix_determinant_lu") % info;

  // since FORTRAN counts from 1, subtract 1 from each of the pivot array values
  for (int i = 0; i < n; i++)
    ipiv[i] -= 1;

  // compute the determinant
  pcps::MantExp<S> det(pcps::unity<S>());
  for (int i = 0; i < n; i++)
    if ( i != ipiv[i] )
      det *= - A[i*n+i];
    else
      det *=   A[i*n+i];

  // return the determinant value
  return det;

}

// explicitly instantiate the function
template pcps::MantExp<double               > pcps::matrix_determinant_lu(const int, double               * const, int * const);
template pcps::MantExp<std::complex<double> > pcps::matrix_determinant_lu(const int, std::complex<double> * const, int * const);
